#include "sba.h"
#include "compiler.h"
#include "demo/eucsbademo.h"
#include "demo/readparams.h"
#include "demo/imgproj.c"
#include "bundleadjustment.h"

void BundleAdjuster::quatMultFast(double q1[FULLQUATSZ], double q2[FULLQUATSZ], double p[FULLQUATSZ]) {
	double t1, t2, t3, t4, t5, t6, t7, t8, t9;

  t1=(q1[0]+q1[1])*(q2[0]+q2[1]);
  t2=(q1[3]-q1[2])*(q2[2]-q2[3]);
  t3=(q1[1]-q1[0])*(q2[2]+q2[3]);
  t4=(q1[2]+q1[3])*(q2[1]-q2[0]);
  t5=(q1[1]+q1[3])*(q2[1]+q2[2]);
  t6=(q1[1]-q1[3])*(q2[1]-q2[2]);
  t7=(q1[0]+q1[2])*(q2[0]-q2[3]);
  t8=(q1[0]-q1[2])*(q2[0]+q2[3]);
  t9=0.5*(t5-t6+t7+t8);
  p[0]= t2 + t9-t5;
  p[1]= t1 - t9-t6;
  p[2]=-t3 + t9-t8;
  p[3]=-t4 + t9-t7;
}


/* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */

/* Given a parameter vector p made up of the parameters of m cameras, compute in
 * hx the prediction of the measurements, i.e. the projections of 3D points in the m images.
 * The measurements are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T,
 * where hx_ij is the predicted projection of the i-th point on the j-th camera.
 * Notice that depending on idxij, some of the hx_ij might be missing
 *
 */
void BundleAdjuster::img_projsKRT_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata)	{ 
  register int i, j;
  int cnp, pnp, mnp;
  double *pqr, *pt, *ppt, *pmeas, *pcalib, *ptparams, *pr0, lrot[FULLQUATSZ], trot[FULLQUATSZ];
  //int n;
  int m, nnz;
  struct globs_ *gl;

  gl=(struct globs_ *)adata;
  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;
  ptparams=gl->ptparams;

  //n=idxij->nr;
  m=idxij->nc;

  for(j=0; j<m; ++j){
    /* j-th camera parameters */
    pcalib=p+j*cnp;
    pqr=pcalib+5;
    pt=pqr+3; // quaternion vector part has 3 elements
    pr0=gl->rot0params+j*FULLQUATSZ; // full quat for initial rotation estimate
    _MK_QUAT_FRM_VEC(lrot, pqr);
    quatMultFast(lrot, pr0, trot); // trot=lrot*pr0

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
	    ppt=ptparams + rcsubs[i]*pnp;
      pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij

      calcImgProjFullR(pcalib, trot, pt, ppt, pmeas); // evaluate Q in pmeas
      //calcImgProj(pcalib, pr0, pqr, pt, ppt, pmeas); // evaluate Q in pmeas
    }
  }
}

/* Given a parameter vector p made up of the parameters of m cameras, compute in jac
 * the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm),
 * where A_ij=dx_ij/db_j (see HZ).
 * Notice that depending on idxij, some of the A_ij might be missing
 *
 */
void BundleAdjuster::img_projsKRT_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) {
  register int i, j, ii, jj;
  int cnp, pnp, mnp, ncK;
  double *pqr, *pt, *ppt, *pA, *pcalib, *ptparams, *pr0;
  //int n;
  int m, nnz, Asz;
  struct globs_ *gl;
  
  gl=(struct globs_ *)adata;
  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;
  ncK=gl->nccalib;
  ptparams=gl->ptparams;

  //n=idxij->nr;
  m=idxij->nc;
  Asz=mnp*cnp;

  for(j=0; j<m; ++j){
    /* j-th camera parameters */
    pcalib=p+j*cnp;
    pqr=pcalib+5;
    pt=pqr+3; // quaternion vector part has 3 elements
    pr0=gl->rot0params+j*FULLQUATSZ; // full quat for initial rotation estimate

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      ppt=ptparams + rcsubs[i]*pnp;
      pA=jac + idxij->val[rcidxs[i]]*Asz; // set pA to point to A_ij

      calcImgProjJacKRT(pcalib, pr0, pqr, pt, ppt, (double (*)[5+6])pA); // evaluate dQ/da in pA

      /* clear the columns of the Jacobian corresponding to fixed calibration parameters */
      if(ncK){
        int jj0;

        jj0=5-ncK;
        for(ii=0; ii<mnp; ++ii, pA+=cnp)
          for(jj=jj0; jj<5; ++jj)
            pA[jj]=0.0; // pA[ii*cnp+jj]=0.0;
      }
    }
  }
}

void BundleAdjuster::bundleAdjust() {
	int	n = 0, /* number of 3D points */
			m = 0, /* number of cameras */
			mcon = 0;  /* number of cameras whose parameters should not be modified */
	char *vmask; /* point visibility mask */
	double *p; /* initial parameter vector P */
	int cnp = 4, /* number of parameters defining a camera, 3 rotations + focal length */
		  pnp = 3; /* euclidean 3D points */
	double *x, /* 
							* measurement vector X, image point and projected image points used to calculate the error
							* x'ij predicted projection of i-th point on the j-th image
							* xij observed image point
							* error vector e = xij - x'ij								
							*/
				 *covx; /* initial estimate? */
	int mnp = 2; /* image points are 2D */
	/*TODO*/int func; /* function computing estimated measurement vector (so calculating error?)*/
	/*TODO*/int fjac; /* function evaulating in sparse Jacobian J */
	void *adata; /* pointer to possibly additional data */
	int itmax = 100, /* maximum number of Levenberg-Marquardt iterations */
			verbose = 0; /* verbosity level */
	double *opts; /* minimisation options for Levenberg-Marquardt */
	double *info; /* info regarding outcome of minimisation */
	
	int n2Dprojs = 1; /* number of 2D projections? */
	double *imgpts = (double *)malloc(n2Dprojs*mnp*sizeof(double)); /* measurement vector x */
  double *covimgpts = (double *)malloc(n2Dprojs*mnp*mnp*sizeof(double)); /* initial estimate? */
	
	x = imgpts;
	covx = covimgpts;

	globs.cnp = cnp;
	globs.pnp = pnp;
	globs.mnp = mnp;

	int	rtn = 0;

  if(argc==4) {
    cout << "4 Arguments for no reason" << endl;
  }
  else {
		cout << "Testin'" << endl;
		cout << "Settings:" << endl;
		cout << "No. of cam params: " << globs.cnp << endl;
		cout << "Euclidean 3D pts:  " << globs.pnp << endl;
		cout << "No. of img pts:    " << globs.mnp << endl;
		rtn = sba_mot_levmar_x(n, m, mcon, vmask, p, cnp, x, covx, mnp,
           img_projsKRT_x,
           img_projsKRT_jac_x,
           (void *)(&globs), itmax, verbose, opts, info);

/* Example usage: 
        n=sba_mot_levmar_x(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, imgpts, covimgpts, mnp,
                          fixedcal? img_projsRT_x : (havedist? img_projsKDRT_x : img_projsKRT_x),
                          analyticjac? (fixedcal? img_projsRT_jac_x : (havedist? img_projsKDRT_jac_x : img_projsKRT_jac_x)) : NULL,
                          (void *)(&globs), MAXITER, verbose, opts, info);
*/
  }

}

void BundleAdjuster::addImageParams() {
}

void BundleAdjuster::
